function plot_slam_laser(true_pose, est_pose, P, true_traj, est_traj, obstacles, landmark_map, time, algorithm_name)
% 激光SLAM通用可视化
% 输入: true_pose-真实位姿, est_pose-估计位姿, P-协方差, true_traj-真实轨迹
%       est_traj-估计轨迹, obstacles-障碍物, landmark_map-路标映射, time-时间, algorithm_name-算法名

    clf;
    
    % 左图：地图+轨迹
    subplot(1, 2, 1); hold on; grid on; axis equal;
    
    % 障碍物
    for i = 1:size(obstacles, 1)
        obs = obstacles(i, :);
        if i <= 4
            rectangle('Position', [obs(1), obs(2), obs(3)-obs(1), obs(4)-obs(2)], ...
                     'FaceColor', [0.3, 0.3, 0.3], 'EdgeColor', 'k', 'LineWidth', 1.5);
        else
            rectangle('Position', [obs(1), obs(2), obs(3)-obs(1), obs(4)-obs(2)], ...
                     'FaceColor', [0.7, 0.7, 0.7], 'EdgeColor', 'k');
        end
    end
    
    % 提取的特征点
    if isa(landmark_map, 'containers.Map') && landmark_map.Count > 0
        lm_keys = cell2mat(keys(landmark_map));
        for i = 1:length(lm_keys)
            lm_idx = landmark_map(lm_keys(i));
            % 路标在状态向量中的位置：3 + (lm_idx-1)*2 + (1:2)
            lm_state_idx = 3 + (lm_idx - 1) * 2 + (1:2);
            
            % 检查索引有效性
            if max(lm_state_idx) <= length(est_pose)
                lm_pos = est_pose(lm_state_idx);
                plot(lm_pos(1), lm_pos(2), 'r*', 'MarkerSize', 8, 'LineWidth', 2);
                
                % 不确定性椭圆
                if size(P, 1) >= max(lm_state_idx) && size(P, 2) >= max(lm_state_idx)
                    P_lm = P(lm_state_idx, lm_state_idx);
                    plot_covariance_ellipse(lm_pos, P_lm, 'r', 0.5);
                end
            end
        end
    end
    
    % 轨迹
    plot(true_traj(1, :), true_traj(2, :), 'g-', 'LineWidth', 2);
    plot(est_traj(1, :), est_traj(2, :), 'b--', 'LineWidth', 1.5);
    
    % 机器人
    draw_robot(true_pose(1), true_pose(2), true_pose(3), 0.8, 'g');
    draw_robot(est_pose(1), est_pose(2), est_pose(3), 0.8, 'b');
    
    xlabel('X(m)'); ylabel('Y(m)');
    if isa(landmark_map, 'containers.Map')
        title(sprintf('%s (t=%.1fs, 特征:%d)', algorithm_name, time, landmark_map.Count));
    else
        title(sprintf('%s (t=%.1fs)', algorithm_name, time));
    end
    xlim([0, 50]); ylim([0, 50]);
    legend('真实轨迹', '估计轨迹', 'Location', 'best');
    
    % 右图：俯视图
    subplot(1, 2, 2); hold on; grid on; axis equal;
    
    for i = 1:size(obstacles, 1)
        obs = obstacles(i, :);
        rectangle('Position', [obs(1), obs(2), obs(3)-obs(1), obs(4)-obs(2)], ...
                 'FaceColor', [0.8, 0.8, 0.8], 'EdgeColor', 'k');
    end
    
    plot(true_traj(1, :), true_traj(2, :), 'g-', 'LineWidth', 1.5);
    plot(est_traj(1, :), est_traj(2, :), 'b--', 'LineWidth', 1.5);
    draw_robot(true_pose(1), true_pose(2), true_pose(3), 1.0, 'g');
    
    xlabel('X(m)'); ylabel('Y(m)');
    title('全局视图');
    xlim([0, 50]); ylim([0, 50]);
    legend('真实', '估计', 'Location', 'best');
end

function draw_robot(x, y, theta, size, color)
    robot_points = size * [0.5, 0; -0.5, 0.3; -0.5, -0.3; 0.5, 0];
    R = [cos(theta), -sin(theta); sin(theta), cos(theta)];
    robot_points = (R * robot_points')';
    robot_points(:, 1) = robot_points(:, 1) + x;
    robot_points(:, 2) = robot_points(:, 2) + y;
    plot(robot_points(:, 1), robot_points(:, 2), color, 'LineWidth', 2);
end

function plot_covariance_ellipse(mu, Sigma, color, alpha)
    % 确保协方差矩阵对称正定
    Sigma = (Sigma + Sigma') / 2;
    Sigma = Sigma + eye(size(Sigma)) * 1e-6;
    
    [V, D] = eig(Sigma);
    
    % 确保特征值为正（数值稳定性）
    D(1,1) = max(D(1,1), 1e-6);
    D(2,2) = max(D(2,2), 1e-6);
    
    angle = atan2(V(2,1), V(1,1));
    a = 2 * sqrt(abs(D(1,1)));  % 使用abs确保安全
    b = 2 * sqrt(abs(D(2,2)));
    theta = linspace(0, 2*pi, 50);
    ellipse = [a*cos(theta); b*sin(theta)];
    R = [cos(angle), -sin(angle); sin(angle), cos(angle)];
    ellipse = R * ellipse;
    plot(mu(1) + ellipse(1,:), mu(2) + ellipse(2,:), color, 'LineWidth', 1);
end

